#pragma once

#include <stddef.h>
#include <Arduino.h>
namespace os
{

template<typename __memory_type,size_t __memory_size,size_t __pool_size,size_t __MaxPoolSize=__pool_size>
class lwmp
{
public:

    typedef struct
    {
        __memory_type data[__memory_size+1];
        size_t size;
    }memory_t;
    typedef struct memory_node
    {
        memory_t memory;
        memory_node* prev;
        memory_node* next;
    }memory_node_t;
    lwmp():_head(&_pool_mem[0]),_tail(_head),_size(__pool_size),_capacity(__pool_size),_allocated_capacity(__pool_size)
    {
        _head->prev = nullptr;
        for(uint32_t i=1;i<(__pool_size);i++){
            memory_node_t* node = &_pool_mem[i];
            node->prev = _tail;
            _tail->next = node;
            _tail = node;
            _tail->next =nullptr;
        }
    }
    ~lwmp()
    {
        #ifdef MBED_CONF_RTOS_PRESENT
        std::lock_guard<globals::mutex::mutex_t>  lck(_mtx);
        #endif
        memory_node_t* node = _head;
        while(node)
        {
            memory_node_t* next = node->next;
            if(node<&_pool_mem[0] || node>&_pool_mem[__pool_size-1]){
                delete node;
            }else{
                //nothing todo
            }
            node = next;
        }
    }

    memory_node_t* alloc()
    {
        #ifdef MBED_CONF_RTOS_PRESENT
        do{
            bool ok = _mtx.trylock();
            if(ok){
                 memory_node_t* mem = this->pop_back();
                if(mem){
                    _mtx.unlock();
                    return mem;
                }
                _mtx.unlock();      
            }
            ThisThread::sleep_for(std::chrono::milliseconds(platform::random()));
        }while(1);
        #else
        return this->pop_back();
        #endif
    }
    void free(memory_node_t* node)
    {
        #ifdef MBED_CONF_RTOS_PRESENT
        do{
            bool ok = _mtx.trylock();
            if(ok){
                this->push_back(node);
                _mtx.unlock();
                return;
            }
            ThisThread::sleep_for(std::chrono::milliseconds(platform::random()));
        }while(1);
        #else
        if(node==nullptr){
            return;
        }
        this->push_back(node);
        #endif
    }
    bool empty()
    {
        return this->_size==0;
    }
    size_t size()
    {
        return _size;
    }
protected:
    void push_back(memory_node_t* node)
    {
        if(this->empty()){
            node->prev = nullptr;
            node->next = nullptr;
            _head = node;
            _tail = node;
            this->_size+=1;
            return;
        }
        node->prev = _tail;
        _tail->next = node;
        _tail = node;
        _tail->next = nullptr;
        _size+=1;
    }
    memory_node_t* pop_back()
    {
        
        memory_node_t* node=nullptr;
        if(empty()){
            if(this->_capacity < __MaxPoolSize){
                node = new memory_node_t;
                if(node!=nullptr){
                    this->_size+=1;
                    this->_capacity+=1;
                    _head = node;
                    _tail = node;
                    _tail->prev =nullptr;
                    _tail->next =nullptr;
                }         
            }
            return nullptr;
        }
        
        this->_size-=1;

        node = _tail;

        

        if(node->prev!=nullptr){
            _tail->prev->next = nullptr;
            _tail = node->prev;
        }else{
            _tail = nullptr;
            _head = nullptr;
        }

        return node;
    }

private:
    memory_node_t _pool_mem[__pool_size];
    memory_node_t* _head;
    memory_node_t* _tail;
    size_t _size;
    size_t _capacity;
    size_t _allocated_capacity;
    #ifdef MBED_CONF_RTOS_PRESENT
    globals::mutex::mutex_t _mtx;
    #endif
};


typedef os::lwmp<uint8_t,32,6> memory_pool_t;

}//namespace os